﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using CrankShaftAnalysisSystem.Devices;

namespace CrankShaftAnalysisSystem.Controller
{
    public class AngleController
    {
        public static double Kp { get; set; } = 200.0;
        public static short Acc { get; set; } = 50;  // 加速

        public static short Decc { get; set; } = 50; // 减速
        public static int VelMin { get; private set; } = 50; // 最小速度
        public static int VelMax { get; private set; } = 800;
        public static double FowardFeed { get; set; } = 1.2; // 前馈

        public static MeasureSystem.State State { get; private set; } = MeasureSystem.State.IDLE;
        public static Task SetPosition(double set_ang, double tol = 0.02)
        {
            return Task.Run(() =>
            {
                if (State == MeasureSystem.State.BUSY)
                {
                    throw new Exception("Angle Controller Busy");
                }
                State = MeasureSystem.State.BUSY;
                if (Math.Abs(set_ang) > Math.PI)
                {
                    Console.WriteLine("Angle Bigger Than Pi, Parse As Degree.");
                    set_ang = set_ang / 180.0 * Math.PI;
                    set_ang = Tools.ReduceRadians(set_ang);
                }
                EncoderERM2400 ERM2400 = EncoderERM2400.Instance;
                PLCMaster PLC = PLCMaster.Instance;
                int vel_setpoint = 0;
                int vel_tmp = 0;
                double brake_ang = 0;
                double acc_rad = Acc / 60.0 * Math.PI * 2; // [rad/s]
                if (!ERM2400.Device.RefFound)
                {
                    throw new Exception("Ref Position Not Fount Yet.");
                }
                while (true)
                {
                    var current_ang = ERM2400.Device.ReadAngleRadians(out _);
                    double brake_time = 1.0/60.0 * vel_setpoint / Acc;
                    brake_ang = 0.5 * acc_rad * brake_time * brake_time;
                    var aditional_ang = 0.0;
                    // 只能朝一个方向转，如果算出来要差值是小于0的，说明当前角度更大
                    // 则角度要变小，电机要正转（注意！电机的正方向和角度编码器的正方向是反的）
                    // 但是结构方面希望电机只能反转，所以需要增加补偿量
                    if (set_ang - current_ang < 0)
                    {
                        aditional_ang = -Math.PI * 2;
                    }
                    else
                        aditional_ang = 0;
                    var pos_err = set_ang - (current_ang + aditional_ang + FowardFeed * brake_ang);

                    if (Math.Abs(current_ang - set_ang) < tol)
                    {
                        Console.WriteLine("Position Reached.");
                        PLC.SetSpeed(1, 0);
                        break;
                    }

                    if (SafeController.ManualStop)
                    {
                        Console.WriteLine("[Safe Controller]: StopAll.");
                        PLC.SetSpeed(1, 0);
                        break;
                    }

                    vel_tmp = (int)(-Kp * pos_err);
                    if (Math.Abs(vel_tmp) < VelMin) vel_tmp = Math.Sign(vel_tmp) * VelMin;
                    if (vel_setpoint < vel_tmp)
                    {
                        vel_setpoint += Decc;
                        if (vel_setpoint > vel_tmp) vel_setpoint = vel_tmp;
                        if (vel_setpoint > 0) vel_setpoint = VelMin;
                    }
                    if (vel_setpoint > vel_tmp)
                    {
                        vel_setpoint -= Acc;
                        if (vel_setpoint < vel_tmp) vel_setpoint = vel_tmp;
                        if (vel_setpoint < -VelMax) vel_setpoint = -VelMax;
                    }
                    PLC.SetSpeed(1, vel_setpoint);
                    Thread.Sleep(150);
                }
                State = MeasureSystem.State.FINISHED;
            });
        }
    }
}
